﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.Geometry;
using FIRADriverLibrary;

namespace RobotRace.engine.agents.implementation
{
    public class PIDPointFollower : AbstractPIDFollower
    {

        public PIDPointFollower(Point2D robotStartingPoint, FIRARobotState controlledRobot, List<Point2D> pathToFollow)
            : base(robotStartingPoint, controlledRobot, pathToFollow)
        {
            this.maxVelocity = 1.0;
            startingStepSpeed = 0.5;
            turningSpeedModifier = 0.2;
            addedErrorWeight = 0.0;
            addedDerivativeWeight = 0.001;
        }

        public override void step()
        {
            if (!robot.owned)
            {
                throw new Exception("ROBOT NOT OWNED!");
            }

            if (path == null || path.Count <= 1)
            {
                return;
            }

            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double currentRobotAngle = robot.angle;
            double currentRobotLeftSpeed = robot.leftSpeed;
            double currentRobotRightSpeed = robot.rightSpeed;

            currentNode = checkpointManagementStrategy.findNextPoint(path, new Point2D(robot.x, robot.y)); // findTargetCheckpoint(new Point2D(currentRobotXPosition, currentRobotYPosition), pathToFollow, currentCheckpoint);

            Vector2D distanceVector = new Vector2D(new Point2D(currentRobotXPosition, currentRobotYPosition), currentNode.point);
            Vector2D robotMoveVector = new Vector2D(currentRobotAngle) *(1 + (currentRobotLeftSpeed + currentRobotRightSpeed) / 2);
            double differenceAngle = Vector2D.AngleBetween(robotMoveVector, distanceVector);            

            //PID start
            robot.leftSpeed = startingStepSpeed;
            robot.rightSpeed = startingStepSpeed;

            sumOfErrors = (2.0 / 3.0) * sumOfErrors + differenceAngle;
            double turn = differenceAngle * turningSpeedModifier +sumOfErrors * addedErrorWeight + (differenceAngle - previousError) * addedDerivativeWeight;
            robot.leftSpeed = robot.leftSpeed + turn;
            robot.rightSpeed = robot.rightSpeed - turn;

            previousError = differenceAngle;
            //PID end

            //make sure we are not going to fast
            obeyMaxVelocityLimit();
        }
    }
}
